
#include "./PinChangeInt.h"
#include "./PinChangeIntConfig.h"

#include "SoftTone.h"
#include "FreqOutT2.h"
#include "TimePin.h"

const int ledStatusPin = 13;
const int tonePin      = 12;

const int irEnableBPin = 9; 
const int irEnableAPin = 8; 
const int irOutPin     = 3; // must be pin 3 (Timer out)

const int irDetectA0Pin = A0;
const int irDetectA1Pin = A1;
const int irDetectB0Pin = A2;
const int irDetectB1Pin = A3;

const long IRFREQ_HZ   = 38e3;
const int  IRFREQ_DUTY = 33;  // percent

void setup()
{
  pinMode( ledStatusPin, OUTPUT);
  pinMode( tonePin,      OUTPUT);

  pinMode( irOutPin,     OUTPUT);
  pinMode( irEnableAPin, OUTPUT);
  pinMode( irEnableBPin, OUTPUT);
  digitalWrite( irEnableAPin, LOW ); // turn off LEDA
  digitalWrite( irEnableBPin, LOW ); // turn off LEDB

  pinMode( irDetectA0Pin,  INPUT);
  pinMode( irDetectA1Pin,  INPUT); 
  pinMode( irDetectB0Pin,  INPUT);
  pinMode( irDetectB1Pin,  INPUT);
  digitalWrite(irDetectA0Pin, HIGH); // internal pullup
  digitalWrite(irDetectA1Pin, HIGH); // internal pullup
  digitalWrite(irDetectB0Pin, HIGH); // internal pullup
  digitalWrite(irDetectB1Pin, HIGH); // internal pullup


  Serial.begin(19200);
  Serial.println("BeamBreak3");

  FreqOutT2_setFreq( IRFREQ_HZ, IRFREQ_DUTY );
  FreqOutT2_on();  // and _off() to turn off

  delay(200);
  playTone( tonePin, NOTE_C1, 50 );
  playTone( tonePin, NOTE_C,  50 );
  playTone( tonePin, NOTE_G,  50 );

  
  for( int i=0; i< 5; i++ ) { 
    digitalWrite( irEnableAPin, LOW );  // turn off LEDA
    delay(50);
    digitalWrite( irEnableAPin, HIGH );  // turn on LEDA
    delay(50);
    digitalWrite( irEnableBPin, LOW );  // turn off LEDB
    delay(50);
    digitalWrite( irEnableBPin, HIGH );  // turn on LEDB
    delay(50);
  }

  PCintPort::attachInterrupt(irDetectA0Pin, detectA, RISING); 
  PCintPort::attachInterrupt(irDetectA1Pin, detectA, RISING); 
  PCintPort::attachInterrupt(irDetectB0Pin, detectB, RISING); 
  PCintPort::attachInterrupt(irDetectB1Pin, detectB, RISING); 

}

void loop()
{
  loop6();
}

volatile unsigned long detectAMillis;
volatile unsigned long detectBMillis;

// first sensor ring
void detectA()
{
  detectAMillis = millis();
}

// second sensor ring
void detectB()
{
  detectBMillis = millis();
}

void loop6()
{
  if( detectAMillis ) { 
    digitalWrite( ledStatusPin, HIGH);
    Serial.print("detectAMillis: "); Serial.println(detectAMillis);
    //playTone( tonePin, NOTE_C, 50 );
    //detectAMillis = 0;
  }
  if( detectBMillis ) {
    int diff = abs(detectBMillis - detectAMillis);
    if( detectAMillis == 0 ) {
      Serial.println("***oops, no detectA");
    }
    digitalWrite( irEnableAPin, LOW );  // turn off LEDA
    digitalWrite( irEnableBPin, LOW );  // turn off LEDB

    Serial.print("detectBMillis: "); Serial.println(detectBMillis);
    Serial.print("diff: "); Serial.println( diff );

    detectAMillis = 0;
    detectBMillis = 0;

    digitalWrite( ledStatusPin, LOW);
    playTone( tonePin, NOTE_C1, 50 );

    digitalWrite( irEnableAPin, HIGH );  // turn on LEDA
    digitalWrite( irEnableBPin, HIGH );  // turn on LEDB
  }

}



/*
void loop5()
{

  digitalWrite( irEnableAPin, HIGH ); // turn on LEDA
  digitalWrite( ledStatusPin, HIGH);

  //int val = pulseIn( irDetectAPin, HIGH, 1000 );
  int val = timePin( irDetectA0Pin, HIGH );
  if( val ) {
    Serial.print( val ); Serial.print(':');
    Serial.println( val/1000 );
  }

  digitalWrite(ledStatusPin, LOW);
  //if( val ) {
  //  Serial.println(val);
  // }
}

void loop4()
{

  digitalWrite( irEnableAPin, HIGH ); // turn on LEDA
  // attach a PinChange Interrupt to our pin on the rising edge
  PCintPort::attachInterrupt(irDetectAPin, detectA, RISING); 
  Serial.println("led A on");
  delay(3000);

  digitalWrite( irEnableAPin, LOW ); // turn off LEDA
  PCintPort::detachInterrupt(irDetectAPin);
  Serial.println("led A off");
  delay(3000);

}

volatile long detectStart;
volatile long detectEnd;
void detectA()
{
  //Serial.println("detectA!");
  //detectStart = millis();
  PCintPort::attachInterrupt(irDetectAPin, detectAend, FALLING); 
}
void detectAend()
{
  detectEnd = millis();
  PCintPort::detachInterrupt(irDetectAPin);
  
}

void loop3()
{

  //irsend.enableIROut(38);
  //irsend.mark(0);  // on

  digitalWrite( irEnableAPin, HIGH ); // turn on LEDA

  int v = digitalRead(irDetectAPin);
  digitalWrite(ledStatusPin, v);

  if( v == 1 ) { // detected
    long start = millis();
    while( v == 1 && ((millis()-start) < 1000) ) { 
      v = digitalRead(irDetectAPin);
    }
    playTone( tonePin, NOTE_C, 50 );
    long dur = millis()- start;
    Serial.println( dur );
  }

  digitalWrite( irEnableAPin, LOW ); // turn off LEDA

  //irsend.space(0); // off

}


*/
